/* -*- Mode: C; indent-tabs-mode: t; c-basic-offset: 4; tab-width: 4 -*- */
/*
 * assign4
 * Copyright (C)  Michael 2008 <mikey.morckos@gmail.com>
 * 
 * assign4 is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * assign4 is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Q1.h"

Q1 :: Q1 () 

: img_height(0),
  img_width(0)

{
	img3 = cvLoadImage("guc1_1058.bmp", 1);
	img4 = cvLoadImage("guc2_1059.bmp", 1);
	
	img1 = cvLoadImage("guc1_1058.bmp", 1);
	img2 = cvLoadImage("guc2_1059.bmp", 1);
	
//	cvCvtColor(img3, img1, CV_BGR2GRAY);
//	cvCvtColor(img4, img2, CV_BGR2GRAY);
	
	img_height = img1->height;
	img_width = img1->width;
	
	
	img_data1.set_IMG(img3);
	img_data2.set_IMG(img4);
	
	convert_Gray();
}

Q1 :: ~Q1() {
	
	cout << "Closing..." << endl;
	
	cvReleaseImage(&img3);
    cvReleaseImage(&img4);	
	cvReleaseImage(&img1);
    cvReleaseImage(&img2);	
}

/* CONVERGION FROM BGR TO GRAY */
void Q1 :: convert_Gray() {
	
	CvScalar s1, s2;
	
	for(int i = 0; i < img_height; i++) { 
		for(int j = 0; j < img_width; j++) {
			s1 = cvGet2D(img1, i, j);
			s2 = cvGet2D(img2, i, j);
			
			s1.val[0] = (uchar)(img_data1[i][j].b * 0.114 + 
                                img_data1[i][j].g * 0.587 + 
                                img_data1[i][j].r * 0.299);
			
			s1.val[1] = (uchar)(img_data1[i][j].b * 0.114 + 
                                img_data1[i][j].g * 0.587 + 
                                 img_data1[i][j].r * 0.299);
			
			s1.val[2] = (uchar)(img_data1[i][j].b * 0.114 + 
                                img_data1[i][j].g * 0.587 + 
                                img_data1[i][j].r * 0.299);
			
			s2.val[0] = (uchar)(img_data2[i][j].b * 0.114 + 
                                img_data2[i][j].g * 0.587 + 
                                img_data2[i][j].r * 0.299);
			
			s2.val[1] = (uchar)(img_data2[i][j].b * 0.114 + 
                                img_data2[i][j].g * 0.587 + 
                                img_data2[i][j].r * 0.299);
			
			s2.val[2] = (uchar)(img_data2[i][j].b * 0.114 + 
                                img_data2[i][j].g * 0.587 + 
                                img_data2[i][j].r * 0.299);
			
			cvSet2D(img1, i, j, s1);
			cvSet2D(img2, i, j, s2);
		}
	}

	img_data1.set_IMG(img1);
	img_data2.set_IMG(img2);
}	

/* APPLYING CORRELATION */
int Q1 :: apply_Correlation (int x1, int y1, int x2, int y2, int flag) {
	
	int result = 0, tmp_val = 0;
	
	tmp_val = (int)img_data1[x1][y1].r - (int)img_data2[x2][y2].r;
	
    /* DEPENDING ON THE FLAG SSD OF SAD IS APPLIED */
	if(flag == 1)
		result += pow(tmp_val, 2);
	else
		result += abs(tmp_val);
	
	if(!(x1 - 1) < 0 && !(y1 - 1) < 0 && !(x2 - 1) < 0 && !(y2 - 1) < 0) {
		tmp_val = (int)img_data1[x1 - 1][y1 - 1].r -
		          (int)img_data2[x2 - 1][y2 - 1].r;
		
		if(flag)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(x1 - 1) < 0 && !(x2 - 1) < 0) {
		tmp_val = (int)img_data1[x1 - 1][y1].r -
		          (int)img_data2[x2 - 1][y2].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(x1 - 1) < 0 && !(x2 - 1) < 0 && 
	   !(y1 + 1) == img_width && !(y2 + 1) == img_width) {
		
		tmp_val = (int)img_data1[x1 - 1][y1 + 1].r -
		          (int)img_data2[x2 - 1][y2 + 1].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(y1 - 1) < 0 && !(y2 - 1) < 0) {
		tmp_val = (int)img_data1[x1][y1 - 1].r -
		          (int)img_data2[x2][y2 - 1].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(y1 + 1) == img_width && !(y2 + 1) == img_width) {
		tmp_val = (int)img_data1[x1][y1 + 1].r -
		          (int)img_data2[x2][y2 + 1].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(x1 + 1) == img_height && !(x2 + 1) == img_height && 
	   !(y1 - 1) < 0 && !(y2 - 1) < 0) {
		
		tmp_val = (int)img_data1[x1 + 1][y1 - 1].r -
		          (int)img_data2[x2 + 1][y2 - 1].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(x1 + 1) == img_height && !(x2 + 1) == img_height) {
		tmp_val = (int)img_data1[x1 + 1][y1].r -
		          (int)img_data2[x2 + 1][y2].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}
	if(!(x1 + 1) == img_height && !(x2 + 1) == img_height && 
	   !(y1 + 1) == img_width && !(y2 + 1) == img_width) {
		
		tmp_val = (int)img_data1[x1 + 1][y1 + 1].r -
		          (int)img_data2[x2 + 1][y2 + 1].r;
		
		if(flag == 1)
			result += pow(tmp_val, 2);
		else
			result += abs(tmp_val);
	}

	return result;
}

void Q1 :: match(int flag) {
	
	Pix_Pos pix_pos[img_height][img_width];
	/* THE MINIMUM RESULT OF SSD OR SAD */
	int min = 0, tmp = 0, tmp_i = 0, tmp_j = 0;
	
	for(int i = 0; i < img_height; i++) {
		for(int j = 0; j < img_width; j++) {
			min = apply_Correlation(i, j, i, j, flag);
			
			for(int m = i - 5; m <= i + 5; m++) {
				for(int n = j - 5; n <= j + 5; n++) {
					if(!(m == img_height) && !(m < 0) &&
					   !(n == img_width) && !(n < 0)) {
				            tmp = apply_Correlation(i, j, m, n, 1);
		               		if(tmp < min) {
			                   	min = tmp;
			                   	tmp_i = m;
			                 	tmp_j = n;
			                }	
				    }
				}	
			}
			if(!(min == 0)) {
				pix_pos[i][j].set_X(tmp_i);
				pix_pos[i][j].set_Y(tmp_j);
			}
			else {
				pix_pos[i][j].set_X(-1);
				pix_pos[i][j].set_Y(-1);
			}	
		}
	}

    for(int i = 0; i < img_height; i++) {
		for(int j = 0; j < img_width; j++) {
			pix_pos[i][j].get_X();
			pix_pos[i][j].get_Y();

			if(!(pix_pos[i][j].get_X() == -1) && !(pix_pos[i][j].get_Y() == -1)) {
				last_x = i - pix_pos[i][j].get_X();
				last_y = j - pix_pos[i][j].get_Y();						   
			}
		}
	}

	cout << "Moves : " << last_x << " in x direction and " 
    << last_y << " in y direction." << endl;	
}

/* MAIN  */
int main (int argc, char *argv[]) {

    Q1 q1;
    int x = 0;
    
    cout << "Type 1 for applying SSD or 0 for SAD : " << endl;
    cin >> x;
    
    if(x == 1)
		q1.match(1);
    else
	    q1.match(0);

    return 0;
}

